/**
 * 
 */
package com.fuzzknuckle.multimixconfig;

import java.awt.geom.Point2D;
import java.util.ArrayList;

/**
 * @author jeff
 *
 */
public class MultiConfig {

	private ArrayList<MultiArm> arms;
	private double scaleFactor = 1;
	
	public MultiConfig() {
		arms = new ArrayList<MultiArm>();
	}
	
	public void addArm(MultiArm arm) {
		this.arms.add(arm);
		
		double avg = (Math.abs(arm.getMotorX()) + Math.abs(arm.getMotorY())) / 2.0;
		if (this.getNumberOfArms() == 1) {
			this.scaleFactor = avg;
		} else {
			this.scaleFactor = (this.scaleFactor + avg) / 2.0;
		}
	} 
	
	public int getNumberOfArms() {
		return this.arms.size();
	}

	public Point2D.Double getOrigin(int motorIndex) {
		if (motorIndex > this.getNumberOfArms()-1) {
			return new Point2D.Double(0.0,0.0);
		} else {
			return new Point2D.Double(arms.get(motorIndex).getOriginX(), arms.get(motorIndex).getOriginX());
		}
	}

	public Point2D.Double getMotor(int motorIndex) {
		if (motorIndex > this.getNumberOfArms()-1) {
			return new Point2D.Double(0.0,0.0);
		} else {
			return new Point2D.Double(arms.get(motorIndex).getMotorX(), arms.get(motorIndex).getMotorX());
		}
	}

	@Override
	public String toString() {
		return "MultiConfig [arms=" + arms + "]";
	}
	
	public String getCMix(int motorIndex) {
		double roll = -1.0 * (arms.get(motorIndex).getMotorX() / this.scaleFactor);
		double pitch = -1.0 * (arms.get(motorIndex).getMotorY() / this.scaleFactor);
		double yaw = Math.abs(roll); 
		//reverse direction on every second motor.
		if (motorIndex % 2 == 0) yaw *= -1;
		return String.format("cmix %d 1.0 %.5g %.5g %.5g", motorIndex, roll, pitch, yaw); 
	}
	
}
